/**
*  Polargraph Server for ATMEGA1280+ based arduino boards.
*  Written by Sandy Noble
*  Released under GNU License version 3.
*  http://www.polargraph.co.uk
*  http://code.google.com/p/polargraph/

2012-04-09 Added checksum as method of verifying serial comms.
*/

#include <AFMotor.h>
#include <AccelStepper.h>
#include <Servo.h>
#include <SD.h>
#include <UTFT.h>
#include <ITDB02_Touch.h>
#include "PolargraphLib.h"
#include "EEPROMAnything.h"
#include <EEPROM.h>

/* Stuff for display */
extern uint8_t SmallFont[];
extern uint8_t BigFont[];
UTFT        lcd(ITDB22, 38,39,40,41);
ITDB02_Touch  touch(6,5,4,3,2);
volatile boolean displayTouched = false;
volatile int touchX = 0;
volatile int touchY = 0;

// Precalculated values for working out CRCs
static PROGMEM prog_uint32_t crc_table[16] = {
    0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac,
    0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c,
    0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c,
    0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c
};

boolean usingCrc = false;


int motorStepsPerRev = 800;
float mmPerRev = 95;

int machineWidth = 650;
int machineHeight = 800;


String machineName = "";

float currentMaxSpeed = 800.0;
float currentAcceleration = 400.0;
boolean usingAcceleration = true;

float SUPERFAST_ACCELERATION = 6000;


int startLengthMM = 800;

float mmPerStep = mmPerRev / motorStepsPerRev;
float stepsPerMM = motorStepsPerRev / mmPerRev;

long pageWidth = machineWidth * stepsPerMM;
long pageHeight = machineHeight * stepsPerMM;
long maxLength = 0;

static String rowAxis = "A";
const int INLENGTH = 50;
const char INTERMINATOR = 10;

const String DIRECTION_STRING_LTR = "LTR";
const int SRAM_SIZE = 2048;

static float penWidth = 0.8; // line width in mm

const int stepType = INTERLEAVE;

boolean acceleration = true;

// create motor objects
AF_Stepper afsteppera(motorStepsPerRev, 1);
void forwarda() { afsteppera.onestep(FORWARD, stepType); }
void backwarda() {  afsteppera.onestep(BACKWARD, stepType); }
AccelStepper motorA(forwarda, backwarda);

AF_Stepper afstepperb(motorStepsPerRev, 2);
void forwardb() {  afstepperb.onestep(FORWARD, stepType); }
void backwardb() {  afstepperb.onestep(BACKWARD, stepType); }
AccelStepper motorB(forwardb, backwardb);

Polargraph polargraph(motorA, motorB);

volatile boolean currentlyRunning = true;

static String inCmd;
static String inParam1;
static String inParam2;
static String inParam3;
static String inParam4;

int inNoOfParams;



static int currentRow = 0;

const String READY = "READY_100";
const String RESEND = "RESEND";
const String DRAWING = "DRAWING";

static String readyString = READY;

String lastCommand = "";
boolean lastCommandConfirmed = false;

int roveMaxA = 0;
int roveMinA = 0;
int roveMaxB = 0;
int roveMinB = 0;

boolean roving = false;


const static String COMMA = ",";
const static String CMD_EXEC = "EXEC";
const static String CMD_ACK = "ACK,";

const static String CMD_CHANGELENGTH = "C01";
const static String CMD_CHANGEPENWIDTH = "C02";
const static String CMD_CHANGEMOTORSPEED = "C03";
const static String CMD_CHANGEMOTORACCEL = "C04";
const static String CMD_DRAWPIXEL = "C05";
const static String CMD_DRAWSCRIBBLEPIXEL = "C06";
const static String CMD_DRAWRECT = "C07";
const static String CMD_CHANGEDRAWINGDIRECTION = "C08";
const static String CMD_SETPOSITION = "C09";
const static String CMD_TESTPATTERN = "C10";
const static String CMD_TESTPENWIDTHSQUARE = "C11";
const static String CMD_TESTPENWIDTHSCRIBBLE = "C12";
const static String CMD_PENDOWN = "C13";
const static String CMD_PENUP = "C14";
const static String CMD_DRAWCIRCLEPIXEL = "C16";
const static String CMD_CHANGELENGTHDIRECT = "C17";
const static String CMD_SETMACHINESIZE = "C24";
const static String CMD_SETMACHINENAME = "C25";
const static String CMD_GETMACHINEDETAILS = "C26";
const static String CMD_RESETEEPROM = "C27";
const static String CMD_DRAWDIRECTIONTEST = "C28";
const static String CMD_SETMACHINEMMPERREV = "C29";
const static String CMD_SETMACHINESTEPSPERREV = "C30";
const static String CMD_SETMOTORSPEED = "C31";
const static String CMD_SETMOTORACCEL = "C32";
const static String CMD_MODE_STORE_COMMANDS = "C33";
const static String CMD_MODE_EXEC_FROM_STORE = "C34";
const static String CMD_MODE_LIVE = "C35";
const static String CMD_RANDOM_DRAW = "C36";

const static String CMD_END = ",END";

boolean storeCommands = false;
boolean drawFromStore = false;
String commandFilename = "commands.txt";

// sd card stuff
const int chipSelect = 53;
boolean sdCardInit = false;
// set up variables using the SD utility library functions:
Sd2Card card;
SdVolume volume;
SdFile root;

void setup() 
{
  Serial.begin(57600);           // set up Serial library at 9600 bps
  Serial.print(F("POLARGRAPH ON!"));
  Serial.println();
  
  // init SD card
  initSD();
  initLCD();

  displayMachineSpec();

  polargraph.getMotorA().setMaxSpeed(currentMaxSpeed);
  polargraph.getMotorA().setAcceleration(currentAcceleration);  
  polargraph.getMotorB().setMaxSpeed(currentMaxSpeed);
  polargraph.getMotorB().setAcceleration(currentAcceleration);

  float startLength = ((float) startLengthMM / (float) mmPerRev) * (float) motorStepsPerRev;
  polargraph.getMotorA().setCurrentPosition(startLength);
  polargraph.getMotorB().setCurrentPosition(startLength);

  polargraph.testServoRange();

  readyString = READY;
  establishContact();
  delay(500);
  polargraph.outputAvailableMemory();
  attachInterrupt(0, touchInput, FALLING);
  
  displayFirstMenu();
}

void touchInput()
{
  if (touch.dataAvailable())
  {
    touch.read();
    int firstTouchX = touch.getX();
    int firstTouchY = touch.getY();

    touch.read();
    int secondTouchX = touch.getX();
    int secondTouchY = touch.getY();
    
    if (firstTouchX == secondTouchX && firstTouchY == secondTouchY)
    {
      touchX = firstTouchX;
      touchY = firstTouchY;
      displayTouched = true;
      lcd.drawPixel(touchX, touchY);
    }
    
//    processTouchCommand();
  }
}




void establishContact() 
{
  ready();
}
void ready()
{
  Serial.println(READY);
}
void drawing()
{
  Serial.println(DRAWING);
}
void acknowledge(String command)
{
  Serial.print(CMD_ACK);
  Serial.println(command);
}

void loop()
{
  checkForInput();
  updateDisplay();

  // send ready
  // wait for instruction
  int idleTime = millis();
  while (!lastCommandConfirmed)
  {
    
    checkForInput();
    updateDisplay();
//    rove();
    // idle
    String inS = "";

    // get incoming command
    while (inS.length() == 0)
    {
//      rove();
      checkForInput();
      updateDisplay();
      int timeSince = millis() - idleTime;
      if (timeSince > 5000)
      {
        ready();
        idleTime = millis();
      }
      inS = readCommand();
    }

    // if it's using the CRC check, then confirmation is easy
    if (usingCrc)
    {
      if (!lastCommandConfirmed)
      {
        requestResend();
      }
      else
      {
        lastCommand = inS;
      }
    }
    else
    {
      // not using CRCs.
      if (inS.equals(CMD_EXEC)) // this is confirming the previous command
      {
        // this just got real
        lastCommandConfirmed = true;
        Serial.print(F("Command confirmed: "));
        Serial.println(lastCommand);
        idleTime = millis();
      }
      else if (inS.startsWith("CANCEL")) // this is cancelling the previous command
      {
        lastCommand = "";
        lastCommandConfirmed = false;
        ready();
        idleTime = millis();
      }
      else // new command
      {
        lastCommand = inS;
        lastCommandConfirmed = false;
        acknowledge(lastCommand);
        idleTime = millis();
      }
    }
  }

  boolean commandParsed = parseCommand(lastCommand);
  if (commandParsed)
  {
    echoLastCommandToDisplay(lastCommand);
    // check for change mode commands
    if (lastCommand.startsWith(CMD_MODE_STORE_COMMANDS)
    || lastCommand.startsWith(CMD_MODE_LIVE))
    {
      Serial.println("Changing mode.");
      Serial.println(F("Executing command."));
      executeCommand(lastCommand);
    }
    // else execute / store the command
    else if (storeCommands)
    {
      Serial.print(F("Storing command:"));
      Serial.println(lastCommand);
      storeCommand(lastCommand);
    }
    else
    {
      Serial.println(F("Executing command."));
      executeCommand(lastCommand);
    }
    lastCommand = "";
    lastCommandConfirmed = false;
    ready();
  }
  else
  {
    Serial.println(F("Command not parsed."));
    lastCommand = "";
    lastCommandConfirmed = false;
    ready();
  }
}


void requestResend()
{
  Serial.println(RESEND);
}
String extractCommandFromExecute(String inS)
{
  String result = inS.substring(8);
  return result;
}


void executeCommand(String inS)
{
  polargraph.outputAvailableMemory();
  
  if (inS.startsWith(CMD_CHANGELENGTH))
  {
    changeLength();
  }
  else if (inS.startsWith(CMD_CHANGELENGTHDIRECT))
  {
    polargraph.changeLengthDirect(asFloat(inParam1), asFloat(inParam2), asInt(inParam3));
  }
  else if (inS.startsWith(CMD_CHANGEPENWIDTH))
  {
    changePenWidth();
  }
  else if (inS.startsWith(CMD_CHANGEMOTORSPEED))
  {
    changeMotorSpeed();
  }
  else if (inS.startsWith(CMD_CHANGEMOTORACCEL))
  {
    changeMotorAcceleration();
  }
  else if (inS.startsWith(CMD_SETMOTORSPEED))
  {
    setMotorSpeed();
  }
  else if (inS.startsWith(CMD_SETMOTORACCEL))
  {
    setMotorAcceleration();
  }
  else if (inS.startsWith(CMD_DRAWPIXEL))
  {
    // go to coordinates.
    drawSquarePixel();
  }  
  else if (inS.startsWith(CMD_DRAWSCRIBBLEPIXEL))
  {
    // go to coordinates.
    drawScribblePixel();
  }
  else if (inS.startsWith(CMD_DRAWCIRCLEPIXEL))
  {
    // draw round pixel
    drawCircularPixel();
  }
  else if (inS.startsWith(CMD_DRAWRECT))
  {
    // go to coordinates.
    drawRectangle();
  }
  else if (inS.startsWith(CMD_CHANGEDRAWINGDIRECTION))
  {
    changeDrawingDirection();
  }
  else if (inS.startsWith(CMD_SETPOSITION))
  {
    setPosition();
  }
  else if (inS.startsWith(CMD_TESTPATTERN))
  {
    testPattern();
  }
  else if (inS.startsWith(CMD_TESTPENWIDTHSQUARE))
  {
    testPenWidth();
  }
  else if (inS.startsWith(CMD_TESTPENWIDTHSCRIBBLE))
  {
    testPenWidthScribble();
  }
  else if (inS.startsWith(CMD_PENDOWN))
  {
    polargraph.penDown();
  }
  else if (inS.startsWith(CMD_PENUP))
  {
    polargraph.penUp();
  }
  else if (inS.startsWith(CMD_SETMACHINESIZE))
  {
    setMachineSizeFromCommand();
  }
  else if (inS.startsWith(CMD_SETMACHINENAME))
  {
    setMachineNameFromCommand();
  }
  else if (inS.startsWith(CMD_SETMACHINEMMPERREV))
  {
    setMachineMmPerRevFromCommand();
  }
  else if (inS.startsWith(CMD_SETMACHINESTEPSPERREV))
  {
    setMachineStepsPerRevFromCommand();
  }
  else if (inS.startsWith(CMD_GETMACHINEDETAILS))
  {
    reportMachineSpec();
  }
  else if (inS.startsWith(CMD_RESETEEPROM))
  {
    polargraph.resetEeprom();
  }
  else if (inS.startsWith(CMD_DRAWDIRECTIONTEST))
  {
    drawTestDirectionSquare();
  }
  else if (inS.startsWith(CMD_MODE_STORE_COMMANDS))
  {
    changeToStoreCommandMode();
  }
  else if (inS.startsWith(CMD_MODE_LIVE))
  {
    changeToLiveCommandMode();
  }
  else if (inS.startsWith(CMD_MODE_EXEC_FROM_STORE))
  {
    execFromStore();
  }
  else if (inS.startsWith(CMD_RANDOM_DRAW))
  {
    drawRandom();
  }
  else
  {
    Serial.print(F("Sorry, "));
    Serial.print(inS);
    Serial.print(F(" isn't a command I recognise."));
    Serial.println();
    ready();
  }

//  Serial.println("After execute:");
//  polargraph.outputAvailableMemory();
  
}

boolean parseCommand(String inS)
{
  String endChars = inS.substring(inS.length() - 4);
  if (endChars.equals(CMD_END))
  {
    extractParams(inS);
    return true;
  }
  else
    return false;
}  

String readCommand()
{
  // check if data has been sent from the computer:
  char inString[INLENGTH+1];
  int inCount = 0;
  while (Serial.available() > 0)
  {
    char ch = Serial.read();       // get it
    delay(15);
    inString[inCount] = ch;
    if (ch == INTERMINATOR)
    {
      Serial.flush();
      break;
    }
    inCount++;
  }
  inString[inCount] = 0;                     // null terminate the string
  String inS = inString;

  // check the CRC for this command
  // and set commandConfirmed true or false
  int colonPos = inS.lastIndexOf(":");
  if (colonPos != -1)
  {
    usingCrc = true;
    String cs = inS.substring(colonPos+1);
    long checksum = asLong(cs);
    inS = inS.substring(0, colonPos);
    
    long calcCrc = crc_string(inS);
    
    if (calcCrc == checksum)
    {
      lastCommandConfirmed = true;
    }
    else
    {
      Serial.print(F("Checksum not matched!:"));
      Serial.println(calcCrc);
      lastCommandConfirmed = false;
    }
  }
  else
  {
    // then fall back and do the ACK - no action here
    usingCrc = false;
    lastCommandConfirmed = false;
  }

  return inS;
}

long asLong(String inParam)
{
  char paramChar[inParam.length() + 1];
  inParam.toCharArray(paramChar, inParam.length() + 1);
  return atol(paramChar);
}
int asInt(String inParam)
{
  char paramChar[inParam.length() + 1];
  inParam.toCharArray(paramChar, inParam.length() + 1);
  return atoi(paramChar);
}
byte asByte(String inParam)
{
  int i = asInt(inParam);
  return (byte) i;
}
float asFloat(String inParam)
{
  char paramChar[inParam.length() + 1];
  inParam.toCharArray(paramChar, inParam.length() + 1);
  return atof(paramChar);
}

/****************************************************************************************************************/
/****************************************************************************************************************/
/****************************************************************************************************************/
/****************************************************************************************************************/
/****************************            BELOW IS THE CODE THAT DOES THE WORK      ******************************/
/****************************************************************************************************************/
/****************************************************************************************************************/
/****************************************************************************************************************/
/****************************************************************************************************************/


void drawRandom()
{
  for (int i = 0; i < 1000; i++)
  {
    Serial.print("Drawing:");
    Serial.println(i);
    while (polargraph.getMotorA().distanceToGo() != 0 && polargraph.getMotorB().distanceToGo() != 0)
    {
      polargraph.getMotorA().run();
      polargraph.getMotorB().run();
    }

    if (polargraph.getMotorA().distanceToGo() == 0)
    {
      int r = random(-2,3);
      polargraph.getMotorA().move(r);

      Serial.print("Chosen new A target: ");
      Serial.println(r);
    }

    if (polargraph.getMotorB().distanceToGo() == 0)
    {
      int r = random(-2,3);
      polargraph.getMotorB().move(r);
      Serial.print("Chosen new B target: ");
      Serial.println(r);
    }
    
    polargraph.reportPosition();
  }
}

void execFromStore()
{
  String fileToExec = inParam1;
  if (fileToExec != "")
  {
    Serial.print("Filename to read from: ");
    Serial.println(fileToExec);
    commandFilename = fileToExec;
    char filename[commandFilename.length()+1];
    commandFilename.toCharArray(filename, commandFilename.length()+1);
    Serial.print("Array to read from: ");
    Serial.println(filename);
    File readFile = SD.open(filename, FILE_READ);
    if (readFile)
    {
      String command = "";
      while (readFile.available())
      {
        char ch = readFile.read();
//        Serial.print(".");
//        Serial.print(ch);
//        Serial.print("-");
        if (ch == 13)
        {
//          Serial.println("cr");
          // execute the line
          boolean commandParsed = parseCommand(command);
          Serial.print(F("Executing command "));
          Serial.println(command);
          executeCommand(command);
          command = "";
        }
        else if (ch == 10)
        {
//          Serial.println("lf");
        }
        else
          command += ch;
      }
    }
  }
  else
  {
    Serial.println("No filename supplied to write to.");
  }
  
}

void changeToStoreCommandMode()
{
  String newfilename = inParam1;
  String newFile = inParam2;
  if (newfilename != "")
  {
    Serial.print("Filename for command store: ");
    Serial.println(newfilename);
    storeCommands = true;
    commandFilename = newfilename;
    if (newFile.equals("R"))
    {
      // delete file if it exists
      char filename[newfilename.length()+1];
      newfilename.toCharArray(filename, newfilename.length()+1);
      
      if (SD.exists(filename))
      {
        // file exists
        Serial.println(F("File already exists."));
        boolean removed = SD.remove(filename);
        if (removed)
          Serial.println(F("File removed."));
        
      }
    }
  }
  else
  {
    Serial.println("No filename supplied to write to.");
  }
}

void changeToLiveCommandMode()
{
  Serial.println(F("Changing back to live mode."));
  storeCommands = false;
}

void reportMachineSpec()
{
  polargraph.dumpEeprom();
  Serial.print(F("PGNAME,"));
  Serial.print(machineName);
  Serial.println(CMD_END);
  
  Serial.print(F("PGSIZE,"));
  Serial.print(machineWidth);
  Serial.print(COMMA);
  Serial.print(machineHeight);
  Serial.println(CMD_END);

  Serial.print(F("PGMMPERREV,"));
  Serial.print(mmPerRev);
  Serial.println(CMD_END);

  Serial.print(F("PGSTEPSPERREV,"));
  Serial.print(motorStepsPerRev);
  Serial.println(CMD_END);
}

void setMachineSizeFromCommand()
{
  int width = asLong(inParam1);
  int height = asLong(inParam2);
  
  if (width > 10)
  {
    polargraph.setWidthMm(width);
  }
  if (height > 10)
  {
    polargraph.setHeightMm(height);
  }
}
void setMachineNameFromCommand()
{
  String name = inParam1;
  polargraph.setName(name);
}

void setMachineMmPerRevFromCommand()
{
  int mmPerRev = asInt(inParam1);
  polargraph.setMmPerRev(mmPerRev);
}
void setMachineStepsPerRevFromCommand()
{
  int stepsPerRev = asInt(inParam1);
  polargraph.setStepsPerRev(stepsPerRev);
}

void setMotorSpeed()
{
  polargraph.setMotorMaxSpeed(asFloat(inParam1));
}
void changeMotorSpeed()
{
  float speedChange = asFloat(inParam1);
  float newSpeed = polargraph.getMotorMaxSpeed() + speedChange;
  polargraph.setMotorMaxSpeed(newSpeed);
 }
 
void setMotorAcceleration()
{
  polargraph.setMotorAcceleration(asFloat(inParam1));
}
void changeMotorAcceleration()
{
  float speedChange = asFloat(inParam1);
  float newAccel = polargraph.getMotorAcceleration() + speedChange;
  polargraph.setMotorAcceleration(newAccel);
}

void changePenWidth()
{
  polargraph.setPenWidth(asFloat(inParam1));
 }   

void changeDrawingDirection() 
{
  polargraph.globalDrawDirectionMode = asInt(inParam1);
  polargraph.globalDrawDirection = asInt(inParam2);
  Serial.print(F("Changed draw direction mode to be "));
  Serial.print(polargraph.globalDrawDirectionMode);
  Serial.print(F(" and direction is "));
  Serial.println(polargraph.globalDrawDirection);
}

  void extractParams(String inS) {
    
    // get number of parameters
    // by counting commas
    int length = inS.length();
    
    int startPos = 0;
    int paramNumber = 0;
    for (int i = 0; i < length; i++) {
      if (inS.charAt(i) == ',') {
        String param = inS.substring(startPos, i);
        startPos = i+1;
        
        switch(paramNumber) {
          case 0:
            inCmd = param;
            break;
          case 1:
            inParam1 = param;
            break;
          case 2:
            inParam2 = param;
            break;
          case 3:
            inParam3 = param;
            break;
          case 4:
            inParam4 = param;
            break;
          default:
            break;
        }
        paramNumber++;
      }
    }
    inNoOfParams = paramNumber;
    
//    Serial.print(F("Command:"));
//    Serial.print(inCmd);
//    Serial.print(F(", p1:"));
//    Serial.print(inParam1);
//    Serial.print(F(", p2:"));
//    Serial.print(inParam2);
//    Serial.print(F(", p3:"));
//    Serial.print(inParam3);
//    Serial.print(F(", p4:"));
//    Serial.println(inParam4);
  }
  
  void testPattern()
  {
    int rowWidth = asInt(inParam1);
    int noOfIncrements = asInt(inParam2);

    boolean ltr = true;
    
    for (int w = rowWidth; w < (w+(noOfIncrements*5)); w+=5)
    {
      for (int i = 0;  i <= polargraph.maxDensity(penWidth, w); i++)
      {
        polargraph.drawSquarePixel(w, w, i, ltr);
      }
      if (ltr)
        ltr = false;
      else
        ltr = true;
        
      polargraph.moveB(w);
    }
  }
  
  void testPenWidth()
  {
    int rowWidth = asInt(inParam1);
    float startWidth = asFloat(inParam2);
    float endWidth = asFloat(inParam3); 
    float incSize = asFloat(inParam4);
    polargraph.testPenWidth(rowWidth, startWidth, endWidth, incSize);
  }
  void testPenWidthScribble()
  {
    int rowWidth = asInt(inParam1);
    float startWidth = asFloat(inParam2);
    float endWidth = asFloat(inParam3); 
    float incSize = asFloat(inParam4);
    polargraph.testPenWidthScribble(rowWidth, startWidth, endWidth, incSize, 0.0);
  }

  void drawRectangle()
  {
  }



void changeLength()
{
  long lenA = asLong(inParam1);
  long lenB = asLong(inParam2);
  
  polargraph.changeLength(lenA, lenB);
}  





void drawTestDirectionSquare()
{
  int rowWidth = asInt(inParam1);
  int segments = asInt(inParam2);
//  polargraph.drawSquarePixel(rowWidth, rowWidth, segments, DIR_SE);
//  polargraph.moveA(rowWidth*2);
//  
//  polargraph.drawSquarePixel(rowWidth, rowWidth, segments, DIR_SW);
//  polargraph.moveB(rowWidth*2);
//  
//  polargraph.drawSquarePixel(rowWidth, rowWidth, segments, DIR_NW);
//  polargraph.moveA(0-(rowWidth*2));
//  
//  polargraph.drawSquarePixel(rowWidth, rowWidth, segments, DIR_NE);
//  polargraph.moveB(0-(rowWidth*2));
  
}


void drawCircularPixel() 
{
    long originA = asLong(inParam1);
    long originB = asLong(inParam2);
    int size = asInt(inParam3);
    int brightness = asInt(inParam4);
    polargraph.drawSpiralAtPosition(originA, originB, size, brightness);
}


void drawSquarePixel() 
{
    long originA = asLong(inParam1);
    long originB = asLong(inParam2);
    int size = asInt(inParam3);
    int density = asInt(inParam4);
    polargraph.drawSquarePixelAtPosition(originA, originB, size, density);
}


void drawScribblePixel() 
{
    int originA = asInt(inParam1);
    int originB = asInt(inParam2);
    int size = asInt(inParam3);
    int brightness = asInt(inParam4);
    polargraph.drawScribblePixelAtPosition(originA, originB, size, brightness, 0);
    polargraph.outputAvailableMemory(); 
}







void setPosition()
{
  long targetA = asInt(inParam1);
  long targetB = asInt(inParam2);

  polargraph.setPosition(targetA, targetB);
  polargraph.engageMotors();
  polargraph.reportPosition();
}








/*  ==============================================================
    Processing the SD card if there is one attached. 
=================================================================*/

void initSD()
{
  pinMode(chipSelect, OUTPUT); // necessary for SD card reading to work
  // see if the card is present and can be initialized:

  // see if the card is present and can be initialized:
  if (!SD.begin(chipSelect)) {
    Serial.println("Card failed, or not present");
    // don't do anything more:
    return;
  }
  Serial.println("card initialized.");

  Serial.println("Initializing SD card...");
  // On the Ethernet Shield, CS is pin 4. It's set as an output by default.
  // Note that even if it's not used as the CS pin, the hardware SS pin 
  // (10 on most Arduino boards, 53 on the Mega) must be left as an output 
  // or the SD library functions will not work. 

  // we'll use the initialization code from the utility libraries
  // since we're just testing if the card is working!
  if (!card.init(SPI_HALF_SPEED, chipSelect)) 
  {
    Serial.println("initialization failed. Things to check:");
    Serial.println("* is a card is inserted?");
    Serial.println("* Is your wiring correct?");
    Serial.println("* did you change the chipSelect pin to match your shield or module?");
    return;
  } 
  else 
  {
   Serial.println("Wiring is correct and a card is present."); 
  }

  // print the type of card
  Serial.print("\nCard type: ");
  switch(card.type()) {
    case SD_CARD_TYPE_SD1:
      Serial.println("SD1");
      break;
    case SD_CARD_TYPE_SD2:
      Serial.println("SD2");
      break;
    case SD_CARD_TYPE_SDHC:
      Serial.println("SDHC");
      break;
    default:
      Serial.println("Unknown");
  }

  // Now we will try to open the 'volume'/'partition' - it should be FAT16 or FAT32
  if (!volume.init(card)) {
    Serial.println("Could not find FAT16/FAT32 partition.\nMake sure you've formatted the card");
    return;
  }


  // print the type and size of the first FAT-type volume
  uint32_t volumesize;
  Serial.print("\nVolume type is FAT");
  Serial.println(volume.fatType(), DEC);
  Serial.println();
  
  volumesize = volume.blocksPerCluster();    // clusters are collections of blocks
  volumesize *= volume.clusterCount();       // we'll have a lot of clusters
  volumesize *= 512;                            // SD card blocks are always 512 bytes
  Serial.print("Volume size (bytes): ");
  Serial.println(volumesize);
  Serial.print("Volume size (Kbytes): ");
  volumesize /= 1024;
  Serial.println(volumesize);
  Serial.print("Volume size (Mbytes): ");
  volumesize /= 1024;
  Serial.println(volumesize);

  
  Serial.println("\nFiles found on the card (name, date and size in bytes): ");
  root.openRoot(volume);
  
  // list all files in the card with date and size
  root.ls(LS_R | LS_DATE | LS_SIZE);
}

void storeCommand(String command)
{
  char filename[8];
  commandFilename.toCharArray(filename, commandFilename.length());
  File storeFile = SD.open("comm.txt", FILE_WRITE);
  
  // if the file opened okay, write to it:
  if (storeFile) 
  {
    Serial.print("Writing to file ");
    Serial.println(commandFilename);
    storeFile.println(command);

    // close the file:
    storeFile.close();
    Serial.println("done.");
  } 
  else 
  {
    // if the file didn't open, print an error:
    Serial.print("error opening ");
    Serial.println(commandFilename);
  }  
}

/*
Calculating CRCs.  Incoming commands have these appended as a way
to check quality.
http://www.excamera.com/sphinx/article-crc.html
*/
unsigned long crc_update(unsigned long crc, byte data)
{
    byte tbl_idx;
    tbl_idx = crc ^ (data >> (0 * 4));
    crc = pgm_read_dword_near(crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4);
    tbl_idx = crc ^ (data >> (1 * 4));
    crc = pgm_read_dword_near(crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4);
    return crc;
}

unsigned long crc_string(String s)
{
  unsigned long crc = ~0L;
  for (int i = 0; i < s.length(); i++)
  {
    crc = crc_update(crc, s.charAt(i));
  }
  crc = ~crc;
  return crc;
}


/* =============================================================================
   This is the code for controlling the LCD, menus and the hardware controls
   =============================================================================*/
byte buttonCoords[12][2];
byte  gap = 10;
byte  buttonSize = 60;
byte  grooveSize = 36;

const byte MENU_INITIAL = 0;
const byte MENU_RUNNING = 1;
volatile byte currentMenu = MENU_INITIAL;

byte numberOfMenuButtons = 3;
const byte BUTTON_SET_HOME = 0;
const byte BUTTON_DRAW_FROM_SD = 1;
const byte BUTTON_MORE_RIGHT = 2;
const byte BUTTON_PAUSE_RUNNING = 3;
const byte BUTTON_RESUME_RUNNING = 4;
const byte BUTTON_RESET = 5;
const byte BUTTON_PEN_UP = 6;
const byte BUTTON_PEN_DOWN = 7;
const byte BUTTON_INC_SPEED = 8;
const byte BUTTON_DEC_SPEED = 9;



void processTouchCommand()
{
  // get control that is under the
  byte pressedButton = getWhichButtonPressed(touchX, touchY);
  switch (pressedButton)
  {
    case BUTTON_SET_HOME:
      polargraph.engageMotors();
      break;
    case BUTTON_DRAW_FROM_SD:
      break;
    case BUTTON_MORE_RIGHT:
      break;
    case BUTTON_PAUSE_RUNNING:
      currentlyRunning = false;
      drawButton(BUTTON_RESUME_RUNNING);
      break;
    case BUTTON_RESUME_RUNNING:
      currentlyRunning = true;
      drawButton(BUTTON_PAUSE_RUNNING);
      break;
    case BUTTON_RESET:
      break;
    case BUTTON_PEN_UP:
      polargraph.penUp();
      drawButton(BUTTON_PEN_DOWN);
      break;
    case BUTTON_PEN_DOWN:
      polargraph.penDown();
      drawButton(BUTTON_PEN_UP);
      break;
    case BUTTON_INC_SPEED:
      break;
    case BUTTON_DEC_SPEED:
      break;
  }
}

void setCurrentMenu(byte menu)
{
  currentMenu = menu;
}

void displayFirstMenu()
{
  lcd.setColor(180,180,180);
  setCurrentMenu(MENU_INITIAL);
  drawButtons();
}

/*
This intialises the LCD itself, and builds the map of the button corner coordinates.
*/
void initLCD()
{
  lcd.InitLCD(LANDSCAPE);
  lcd.clrScr();
  lcd.setFont(SmallFont);

  touch.InitTouch(LANDSCAPE);
  touch.setPrecision(PREC_HI);
  
  // output lcd size
//  lcd.printNumI(lcd.getDisplayXSize(), 10, 40);
//  lcd.printNumI(lcd.getDisplayYSize(), 10, 52);


  gap = 10;
  buttonSize = (lcd.getDisplayXSize() - (gap*4)) / 3;
  grooveSize = lcd.getDisplayYSize() - (gap*2) - (buttonSize*2);


  buttonCoords[0][0] = gap;
  buttonCoords[0][1] = gap;
  buttonCoords[1][0] = gap+buttonSize;
  buttonCoords[1][1] = gap+buttonSize;

  buttonCoords[2][0] = gap+buttonSize+gap;
  buttonCoords[2][1] = gap;
  buttonCoords[3][0] = gap+buttonSize+gap+buttonSize;
  buttonCoords[3][1] = gap+buttonSize;

  buttonCoords[4][0] = gap+buttonSize+gap+buttonSize+gap;
  buttonCoords[4][1] = gap;
  buttonCoords[5][0] = gap+buttonSize+gap+buttonSize+gap+buttonSize;
  buttonCoords[5][1] = gap+buttonSize;

  buttonCoords[6][0] = gap;
  buttonCoords[6][1] = gap+buttonSize+grooveSize;
  buttonCoords[7][0] = gap+buttonSize;
  buttonCoords[7][1] = gap+buttonSize+buttonSize+grooveSize;

  buttonCoords[8][0] = gap+buttonSize+gap;
  buttonCoords[8][1] = gap+buttonSize+grooveSize;
  buttonCoords[9][0] = gap+buttonSize+gap+buttonSize;
  buttonCoords[9][1] = gap+buttonSize+buttonSize+grooveSize;

  buttonCoords[10][0] = gap+buttonSize+gap+buttonSize+gap;
  buttonCoords[10][1] = gap+buttonSize+grooveSize;
  buttonCoords[11][0] = gap+buttonSize+gap+buttonSize+gap+buttonSize;
  buttonCoords[11][1] = gap+buttonSize+buttonSize+grooveSize;

  lcd.setBackColor(0,0,0);
  lcd.setColor(255,255,255);
  lcd.print("Polargraph.", 20, buttonCoords[5][1]+10);
  lcd.print("Drawing with robots.", 20, buttonCoords[5][1]+24);
  delay(1000);

}

void drawButtons()
{
  if (currentMenu == MENU_INITIAL)
  {
//    numberOfMenuButtons = 6;
//    drawButtonBackgrounds(numberOfMenuButtons);
    lcd.setBackColor(180,180,180);
    lcd.setColor(0,0,0);

    drawButton(BUTTON_SET_HOME);
    drawButton(BUTTON_DRAW_FROM_SD);
    drawButton(BUTTON_MORE_RIGHT);

    if (currentlyRunning)
      drawButton(BUTTON_PAUSE_RUNNING);
    else
      drawButton(BUTTON_RESUME_RUNNING);
      
    if (polargraph.getPenRaised())
      drawButton(BUTTON_PEN_DOWN);
    else
      drawButton(BUTTON_PEN_UP);
  
    drawButton(BUTTON_RESET);
  }
  else if (currentMenu == MENU_RUNNING)
  {
//    numberOfMenuButtons = 6;
//    drawButtonBackgrounds(numberOfMenuButtons);   
  }
}

const byte COL_LIGHT_R = 180;
const byte COL_LIGHT_G = 180;
const byte COL_LIGHT_B = 180;

const byte COL_DARK_R = 0;
const byte COL_DARK_G = 0;
const byte COL_DARK_B = 0;

void drawButton(byte but)
{
  switch (but)
  {
    case BUTTON_SET_HOME:
      lcd.setColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.fillRect(buttonCoords[0][0], buttonCoords[0][1], buttonCoords[1][0], buttonCoords[1][1]);
      lcd.setBackColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.setColor(COL_DARK_R,COL_DARK_G,COL_DARK_B);
      lcd.print("SET", buttonCoords[0][0]+(buttonSize/2)-10, buttonCoords[0][1]+(buttonSize/2)-12);
      lcd.print("HOME", buttonCoords[0][0]+(buttonSize/2)-14, buttonCoords[0][1]+(buttonSize/2));
      break;
    case BUTTON_DRAW_FROM_SD:
      lcd.setColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.fillRect(buttonCoords[2][0], buttonCoords[2][1], buttonCoords[3][0], buttonCoords[3][1]);
      lcd.setBackColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.setColor(COL_DARK_R,COL_DARK_G,COL_DARK_B);
      lcd.print("DRAW", buttonCoords[2][0]+(buttonSize/2)-14, buttonCoords[2][1]+(buttonSize/2)-18);
      lcd.print("FROM", buttonCoords[2][0]+(buttonSize/2)-14, buttonCoords[2][1]+(buttonSize/2)-6);
      lcd.print("SD", buttonCoords[2][0]+(buttonSize/2)-6, buttonCoords[2][1]+(buttonSize/2)+6);
      break;
    case BUTTON_MORE_RIGHT:
      lcd.setColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.fillRect(buttonCoords[4][0], buttonCoords[4][1], buttonCoords[5][0], buttonCoords[5][1]);
      lcd.setBackColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.setColor(COL_DARK_R,COL_DARK_G,COL_DARK_B);
      lcd.print("MORE", buttonCoords[4][0]+(buttonSize/2)-14, buttonCoords[4][1]+(buttonSize/2)-6);
      break;
      
    case BUTTON_PAUSE_RUNNING:
      lcd.setColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.fillRect(buttonCoords[6][0], buttonCoords[6][1], buttonCoords[7][0], buttonCoords[7][1]);
      lcd.setBackColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.setColor(COL_DARK_R,COL_DARK_G,COL_DARK_B);
      lcd.print("PAUSE", buttonCoords[6][0]+(buttonSize/2)-14, buttonCoords[6][1]+(buttonSize/2)-6);
      break;
    case BUTTON_RESUME_RUNNING:
      lcd.setColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.fillRect(buttonCoords[6][0], buttonCoords[6][1], buttonCoords[7][0], buttonCoords[7][1]);
      lcd.setBackColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.setColor(COL_DARK_R,COL_DARK_G,COL_DARK_B);
      lcd.print("RUN", buttonCoords[6][0]+(buttonSize/2)-10, buttonCoords[6][1]+(buttonSize/2)-6);
      break;
    case BUTTON_RESET:
      lcd.setColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.fillRect(buttonCoords[10][0], buttonCoords[10][1], buttonCoords[11][0], buttonCoords[11][1]);
      lcd.setBackColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.setColor(COL_DARK_R,COL_DARK_G,COL_DARK_B);
      lcd.print("RESET", buttonCoords[10][0]+(buttonSize/2)-18, buttonCoords[10][1]+(buttonSize/2)-6);
      break;
    case BUTTON_PEN_UP:
      lcd.setColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.fillRect(buttonCoords[8][0], buttonCoords[8][1], buttonCoords[9][0], buttonCoords[9][1]);
      lcd.setBackColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.setColor(COL_DARK_R,COL_DARK_G,COL_DARK_B);
      lcd.print("PEN", buttonCoords[8][0]+(buttonSize/2)-10, buttonCoords[8][1]+(buttonSize/2)-12);
      lcd.print("UP", buttonCoords[8][0]+(buttonSize/2)-6, buttonCoords[8][1]+(buttonSize/2));
      break;
    case BUTTON_PEN_DOWN:
      lcd.setColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.fillRect(buttonCoords[8][0], buttonCoords[8][1], buttonCoords[9][0], buttonCoords[9][1]);
      lcd.setBackColor(COL_LIGHT_R,COL_LIGHT_G,COL_LIGHT_B);
      lcd.setColor(COL_DARK_R,COL_DARK_G,COL_DARK_B);
      lcd.print("PEN", buttonCoords[8][0]+(buttonSize/2)-10, buttonCoords[8][1]+(buttonSize/2)-12);
      lcd.print("DOWN", buttonCoords[8][0]+(buttonSize/2)-14, buttonCoords[8][1]+(buttonSize/2));
      break;
    case BUTTON_INC_SPEED:
      break;
    case BUTTON_DEC_SPEED:
      break;
  }
}



//void drawButtonBackgrounds(byte buts)
//{
//  lcd.clrScr();
//  if (buts >= 3)
//  {
//    lcd.fillRect(buttonCoords[0][0], buttonCoords[0][1], buttonCoords[1][0], buttonCoords[1][1]);
//    lcd.fillRect(buttonCoords[2][0], buttonCoords[2][1], buttonCoords[3][0], buttonCoords[3][1]);
//    lcd.fillRect(buttonCoords[4][0], buttonCoords[4][1], buttonCoords[5][0], buttonCoords[5][1]);
//  }
//  
//  if (buts == 6)
//  {
//    lcd.fillRect(buttonCoords[6][0], buttonCoords[6][1], buttonCoords[7][0], buttonCoords[7][1]);
//    lcd.fillRect(buttonCoords[8][0], buttonCoords[8][1], buttonCoords[9][0], buttonCoords[9][1]);
//    lcd.fillRect(buttonCoords[10][0], buttonCoords[10][1], buttonCoords[11][0], buttonCoords[11][1]);
//  }
//}

void displayMachineSpec()
{
  lcd.setColor(0,0,0);
  lcd.fillRect(10, 10, 210, 166);
  lcd.setColor(180,180,180);
  
  const int ROW_HEIGHT = 12;
  int currentRow = 30;
  byte key = 20;
  byte val = 100;
  
  lcd.print("Width: ", key, currentRow);
  lcd.printNumI(machineWidth, val, currentRow);
  
  currentRow += ROW_HEIGHT;
  lcd.print("Height:", key, currentRow);
  lcd.printNumI(machineHeight, val, currentRow);

  currentRow += ROW_HEIGHT;
  lcd.print("MmPerRev:", key, currentRow);
  lcd.printNumF(mmPerRev, 5, val, currentRow);

  currentRow += ROW_HEIGHT;
  lcd.print("StepsPerRev:", key, currentRow);
  lcd.printNumI(motorStepsPerRev, val, currentRow);

  currentRow += ROW_HEIGHT;
  lcd.print("MmPerStep:", key, currentRow);
  lcd.printNumF(mmPerStep,5, val, currentRow);

  currentRow += ROW_HEIGHT;
  lcd.print("StepsPerMM:", key, currentRow);
  lcd.printNumF(stepsPerMM,5, val, currentRow);
  
  currentRow += ROW_HEIGHT;
  lcd.print("PageWidth:", key, currentRow);
  lcd.printNumI(pageWidth, val, currentRow);

  currentRow += ROW_HEIGHT;
  lcd.print("PageHeight:", key, currentRow);
  lcd.printNumI(pageHeight, val, currentRow);


  currentRow += ROW_HEIGHT;
  lcd.print("Name:", key, currentRow);
  lcd.print(machineName, val, currentRow);
  
}

void echoLastCommandToDisplay(String com)
{
  lcd.setColor(0,0,0);
  lcd.fillRect(buttonCoords[0][0],buttonCoords[1][1]+10, 220, buttonCoords[1][1]+24);
  lcd.setColor(180,180,180);
  lcd.setBackColor(0,0,0);
  lcd.print(com, buttonCoords[0][0],buttonCoords[1][1]+10);
}

byte getWhichButtonPressed(int x, int y)
{
  if (x >= buttonCoords[0][0] && x <= buttonCoords[1][0] 
     && y >= buttonCoords[0][1] && y <= buttonCoords[1][1])
    return BUTTON_SET_HOME;
  else if (x >= buttonCoords[2][0] && x <= buttonCoords[3][0] 
     && y >= buttonCoords[2][1] && y <= buttonCoords[1][1])
    return BUTTON_DRAW_FROM_SD;   
  else if (x >= buttonCoords[4][0] && x <= buttonCoords[5][0] 
     && y >= buttonCoords[4][1] && y <= buttonCoords[5][1])
    return BUTTON_MORE_RIGHT;   

  else if (x >= buttonCoords[6][0] && x <= buttonCoords[7][0] 
     && y >= buttonCoords[6][1] && y <= buttonCoords[7][1])
   {
     if (currentlyRunning)
       return BUTTON_PAUSE_RUNNING;
     else
       return BUTTON_RESUME_RUNNING;   
   }
  else if (x >= buttonCoords[8][0] && x <= buttonCoords[9][0] 
     && y >= buttonCoords[8][1] && y <= buttonCoords[9][1])
   {
     if (polargraph.getPenRaised())
       return BUTTON_PEN_DOWN;
     else
       return BUTTON_PEN_UP;   
   }
  else if (x >= buttonCoords[10][0] && x <= buttonCoords[11][0] 
     && y >= buttonCoords[10][1] && y <= buttonCoords[11][1])
    return BUTTON_RESET;   
}

void checkForInput()
{
  if (displayTouched)
  {
    lcd.setBackColor(random(0,255),random(0,255),random(0,255));
    lcd.print("Display touched", 10, 10);
    delay(200);
    drawButton(BUTTON_SET_HOME);
    drawButton(BUTTON_DRAW_FROM_SD);
    processTouchCommand();
    displayTouched = false;
  }
}
void updateDisplay()
{
}

